//*****************************************************************************
//
// pwmgen.c - PWM signal generation example.
//
// Copyright (c) 2008-2012 Texas Instruments Incorporated.  All rights reserved.
// Software License Agreement
// 
// Texas Instruments (TI) is supplying this software for use solely and
// exclusively on TI's microcontroller products. The software is owned by
// TI and/or its suppliers, and is protected under applicable copyright
// laws. You may not combine this software with "viral" open-source
// software in order to form a larger program.
// 
// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS.
// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT
// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY
// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
// DAMAGES, FOR ANY REASON WHATSOEVER.
// 
// This is part of revision 8555 of the EK-LM3S3748 Firmware Package.
//
//*****************************************************************************

#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
#include "inc/hw_nvic.h"
#include "inc/hw_types.h"
#include "inc/hw_qei.h"
#include "inc/hw_timer.h"
#include "driverlib/debug.h"
#include "driverlib/gpio.h"
#include "driverlib/pwm.h"
#include "driverlib/rom.h"
#include "driverlib/sysctl.h"
#include "driverlib/uart.h"
#include "driverlib/interrupt.h"
#include "driverlib/qei.h"
#include "driverlib/timer.h"
#include "stdint.h"
#include "math.h"
#include "stdlib.h"

//*****************************************************************************
//
//! \addtogroup example_list
//! <h1>PWM (pwmgen)</h1>
//!
//! This example application utilizes the PWM peripheral to output a 20% duty
//! cycle PWM signal and a 80% duty cycle PWM signal, both at 8000 Hz.  Once
//! configured, the application enters an infinite loop, doing nothing while
//! the PWM peripheral continues to output its signals.
//
//*****************************************************************************


//*****************************************************************************
//
// Define something
//
//*****************************************************************************
#define MaxVelocityrpm	2000
#define MaxVelocitypps	20000


#define OpperationFrequentcy	20000		//at Hz (minimum at 800 Hz)
#define ReceiveUpdateData		0x01
#define StartMotor				0x02
#define StopMotor				0x04
#define SendCVToPC				0x08


unsigned long ulPeriod;
volatile float u[2];
volatile float e[3];
volatile float tam1 = 0,tam2 = 0;

//*****************************************************************************
//
// Variable used for QEI Module
//
//*****************************************************************************
unsigned long ulQEI0ForwardTimerInt, ulQEI0ReverseTimerInt, ulQEI1ForwardTimerInt, ulQEI1ReverseTimerInt;
volatile long lCurrentPosition;
volatile long lIntervalPulse;
unsigned long QEICycle;


volatile float SetVelocityrpm=0;
volatile float CurrentVelocityrpm=0;
volatile int32_t lSetVelocitypps=0;
volatile int32_t lCurrentVelocitypps=0;


volatile float a0=0, a1=0, a2=0;


//*****************************************************************************
//
// The error routine that is called if the driver library encounters an error.
//
//*****************************************************************************
#ifdef DEBUG
void
__error__(char *pcFilename, unsigned long ulLine)
{
}
#endif

unsigned char ucRunningStatus;

void UARTSendUint32(uint32_t data);
void UARTSendInt32(int32_t data);

void UART0_Init(void)
{
    //
    // Enable the peripherals
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

    //
    // Set GPIO A0 and A1 as UART pins.
    //
    ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

    //
    // Configure the UART for 115,200, 8-N-1 operation.
    //
    ROM_UARTConfigSetExpClk(UART0_BASE, ROM_SysCtlClockGet(), 115200,
                            (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
                             UART_CONFIG_PAR_NONE));

    //
    // Enable the UART interrupt.
    //
    ROM_IntEnable(INT_UART0);
    ROM_UARTIntEnable(UART0_BASE, UART_INT_RX|UART_INT_RT);
}

void PWM0_Init(void)
{

    ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_1);

    //
    // Enable the peripherals.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);

    //
    // Set GPIO F0 and F1 as PWM pins.  They are used to output the PWM0 and
    // PWM1 signals.
    //
    ROM_GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    ROM_GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1,
                         GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);

    //
    // Set GPIO H0 and H1 as PWM pins.  They are used to output the PWM2 and
    // PWM3 signals.
    //
    ROM_GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    ROM_GPIOPadConfigSet(GPIO_PORTH_BASE, GPIO_PIN_0 | GPIO_PIN_1,
                         GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);

    //
    // Compute the PWM period based on the system clock.
    //
    ulPeriod = (ROM_SysCtlClockGet()/OpperationFrequentcy);

    //
    // Set the PWM period
    //
    ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_0,
                        PWM_GEN_MODE_DOWN | PWM_GEN_MODE_SYNC|PWM_GEN_MODE_DBG_RUN);
    ROM_PWMDeadBandEnable(PWM0_BASE,PWM_GEN_0,ROM_SysCtlClockGet()/1000000,ROM_SysCtlClockGet()/1000000);
    ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, ulPeriod);


    //
    // Set PWM0 to a duty cycle of
    //
    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 0);

    //
    // Enable the PWM0 and PWM1 output signals.
    //
    ROM_PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT, true);

    //
    // Set the PWM period gen 1
    //
    ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_1,
                        PWM_GEN_MODE_DOWN | PWM_GEN_MODE_SYNC|PWM_GEN_MODE_DBG_RUN);
    ROM_PWMDeadBandEnable(PWM0_BASE,PWM_GEN_1,ROM_SysCtlClockGet()/1000000,ROM_SysCtlClockGet()/1000000);
    ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, ulPeriod);


    //
    // Set PWM2 to a duty cycle of
    //
    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 0);

    //
    // Enable the PWM2 and PWM3 output signals.
    //
    ROM_PWMOutputState(PWM0_BASE, PWM_OUT_2_BIT | PWM_OUT_3_BIT, true);

    //
    // Enable the PWM generator.
    //
    ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_0);
    ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_1);

    ROM_PWMSyncTimeBase(PWM0_BASE, PWM_GEN_0_BIT|PWM_GEN_1_BIT);

    //synchronize PWM0 Gen0&Gen1
    ROM_PWMSyncUpdate(PWM0_BASE,PWM_GEN_0_BIT|PWM_GEN_1_BIT);
}

void QEI_Init(void)
{
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI);

    //
    // Configure the PC6, PF6 to be QEI0 channel A, B pin
    //
    ROM_GPIOPinTypeQEI(GPIO_PORTF_BASE, GPIO_PIN_6);
    ROM_GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_6);

    //
    //Configure module QEI
    //
    ROM_QEIConfigure(QEI_BASE, QEI_CONFIG_CAPTURE_A_B|QEI_CONFIG_NO_RESET
    		|QEI_CONFIG_QUADRATURE|QEI_CONFIG_NO_SWAP, 500000);
    HWREG(QEI_BASE|QEI_O_CTL) |= (0xF<<16)|(1<<13); //enable noise filter

    //ROM_QEIVelocityConfigure(QEI_BASE, QEI_VELDIV_1, Encoder_Sample_Interval);
    ROM_QEIVelocityConfigure(QEI_BASE, QEI_VELDIV_1, QEICycle);

    ROM_IntEnable(INT_QEI);

    ROM_QEIIntEnable(QEI_BASE, QEI_INTTIMER|QEI_INTDIR);
    ROM_QEIEnable(QEI_BASE);
    ROM_QEIVelocityEnable(QEI_BASE);
    ROM_QEIPositionSet(QEI_BASE, 250000);
}

//*****************************************************************************
//
// Initialize Timer0
// After this API, timer0 don't run yet
// Timer0 run only after using API TimerEnable
//
//*****************************************************************************
void Timer0_Init(uint32_t period)
{
    //
    // Enable the peripherals used by this example.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);

    //
    // Configure the  32-bit periodic timers 0 for ...
    //
    ROM_TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC);
    ROM_TimerLoadSet(TIMER0_BASE, TIMER_A, period);

    //
    // Setup the interrupts for the timer timeouts.
    //
    ROM_IntEnable(INT_TIMER0A);
    ROM_TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
}

void PWM0_ChangeDytyCycle(int32_t DutyCycle)
{
	int32_t lTempDuty;
//	if(labs(DutyCycle) > 0.95*ulPeriod)
//		lTempDuty = 0.95*ulPeriod;
//	else
//		lTempDuty = DutyCycle;

	if (DutyCycle >= 0)
	{
		lTempDuty = DutyCycle;
	}
	else
	{
		lTempDuty = -DutyCycle;
	}

	lTempDuty = ulPeriod - labs(lTempDuty);
	if(DutyCycle < 0)
	{
		//PWM pin0 =1 (low side)
		//pwm pin1 =0 (high side)
		//pwm pin2,3 out pwm
	    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 0);
	    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, (lTempDuty));
	}
	else
	{
		//PWM pin2 =1 (low side)
		//pwm pin3 =0 (high side)
		//pwm pin0,1 out pwm
	    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, (lTempDuty));
	    ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 0);
	}

	//these codes is used with ir2184

    //synchronize PWM0 Gen0&Gen1
    ROM_PWMSyncUpdate(PWM0_BASE,PWM_GEN_0_BIT|PWM_GEN_1_BIT);
}


int32_t PID_Control(void)
{
//
//			tam1 = lSetVelocitypps;
//			tam2 = lCurrentVelocitypps;
//
//			e[2] = tam1 - tam2;
//			u[1] = u[0] + a0*e[2] + a1*e[1] + a2*e[0];
//			if (u[1]>0.95*ulPeriod) u[1]=0.95*ulPeriod;
//			else if(u[1]<(-0.95*ulPeriod))	u[1] = -0.95*ulPeriod;
//			u[0] = u[1];
//			e[0] = e[1];
//			e[1] = e[2];
//			return ((int32_t)u[1]);

	static int err[2];
	static long I;
	static char count_temp = 0;
	int32_t control;

			err[1] = err[0];
			err[0] = lSetVelocitypps - lCurrentVelocitypps;

			I += err[0];
			if (I >= 80) I = 80;
			if (I <= -80) I = -80;

			control = 2*err[0] + 0.3*I + 0.1*(err[1]-err[0]);


			if (control > 1000) control = 1000;
			if (control < -1000) control = -1000;

			return control;
}


//*****************************************************************************
//
// Send a string to the UART.
//
//*****************************************************************************
void
UARTSendAString(const unsigned char *pucBuffer)
{
    //
    // Loop while there are more characters to send.
    //
    while(*pucBuffer != 0)
    {
        //
        // Write the next character to the UART.
        //
        ROM_UARTCharPut(UART0_BASE, *pucBuffer++);
    }
}


//*****************************************************************************
//
// Send a number of data to the UART.
//
//*****************************************************************************
void
UARTSend(const unsigned char *pucBuffer, unsigned long ulCount)
{
    //
    // Loop while there are more characters to send.
    //
    while(ulCount--)
    {
        //
        // Write the next character to the UART.
        //
        ROM_UARTCharPut(UART0_BASE, *pucBuffer++);
    }
}


void UARTSendUint32(uint32_t data)
{
	uint32_t a = data;
	int8_t i;
	uint8_t du, soso;
	uint8_t number[10];
	if(data==0)
		ROM_UARTCharPut(UART0_BASE,0x30);
	for(i=0;i<10;i++)
	{
		du=(a%10);
		a=(uint32_t)(a/10);
		if((du==0)&&(a==0))
		{
			soso=i-1;
			break;
		}
			number[i]=du;
	}
	for(i=soso;i>=0;i--)
	{
		ROM_UARTCharPut(UART0_BASE,number[i]+0x30);
	}
	return;
}

void UARTSendInt32(int32_t data)
{
	int32_t a = data;
	int8_t i;
	uint8_t du, soso;
	uint8_t number[10];
	uint8_t sign;
	if(data==0)
		ROM_UARTCharPut(UART0_BASE,0x30);
	if((data&0x8000)==0x8000)
	{
		sign = 1;
		//convert data to unsigned
		a = - data;
	}
	else
		sign = 0;
	for(i=0;i<10;i++)
	{
		du=(a%10);
		a=(int32_t)(a/10);
		if((du==0)&&(a==0))
		{
			soso=i-1;
			break;
		}
			number[i]=du;
	}
	//send sign of data
	if(sign)
		ROM_UARTCharPut(UART0_BASE,'-');
	for(i=soso;i>=0;i--)
	{
		ROM_UARTCharPut(UART0_BASE,number[i]+0x30);
	}
}




//*****************************************************************************
//
// The UART interrupt handler.
//
//*****************************************************************************
void
UARTIntHandler(void)
{
    unsigned long ulStatus;
    unsigned char ucReceiveChar;

    //
    // Get the interrupt status.
    //
    ulStatus = ROM_UARTIntStatus(UART0_BASE, true);

    //
    // Clear the asserted interrupts.
    //
    ROM_UARTIntClear(UART0_BASE, ulStatus);

    ucReceiveChar = ROM_UARTCharGet(UART0_BASE);
    ROM_UARTCharPut(UART0_BASE,ucReceiveChar);
    switch(ucReceiveChar)
    {
	case 'c':
    	HWREGBITB(&ucRunningStatus,StartMotor)=1;
    	HWREGBITB(&ucRunningStatus,StopMotor)=0;
		break;
	case 'd':
    	HWREGBITB(&ucRunningStatus,StartMotor)=0;
    	HWREGBITB(&ucRunningStatus,StopMotor)=1;
		break;
	case 'r':
    	HWREGBITB(&ucRunningStatus,SendCVToPC)=1;
		break;
    default:
    	break;
    }
}

//*****************************************************************************
// QEI Interrupt Service Routine
//*****************************************************************************

void
QEIIntHandler(void)
{
	int32_t bientam1;
	if((ROM_QEIIntStatus(QEI0_BASE,0)&QEI_INTTIMER)==QEI_INTTIMER)
	{
	//Clear Interrupt timer flag
	ROM_QEIIntClear(QEI0_BASE, QEI_INTTIMER);
	bientam1 = (SysCtlClockGet()/QEICycle);
	lIntervalPulse = ROM_QEIVelocityGet(QEI0_BASE);
	lCurrentPosition = (((long)ROM_QEIPositionGet(QEI0_BASE) - 250000)*bientam1);
	ROM_QEIPositionSet(QEI0_BASE, 250000);
	lCurrentVelocitypps = lCurrentPosition;
/*	if(ROM_QEIDirectionGet(QEI0_BASE)==(-1))
		lCurrentVelocitypps = -(lCurrentPosition*(SysCtlClockGet()/QEICycle));
	else
		lCurrentVelocitypps = (lCurrentPosition*(SysCtlClockGet()/QEICycle));*/
	//Reset Position


	PWM0_ChangeDytyCycle(PID_Control());

//	UARTSendInt32(lCurrentPosition);
//	ROM_UARTCharPut(UART0_BASE,'\r');
//	UARTSendInt32(lIntervalPulse);
//	ROM_UARTCharPut(UART0_BASE,'\r');
	UARTSendInt32(lCurrentVelocitypps);
	ROM_UARTCharPut(UART0_BASE,'\r');
	}
	if((ROM_QEIIntStatus(QEI0_BASE,0)&QEI_INTDIR)==QEI_INTDIR)
	{
		ROM_QEIIntClear(QEI0_BASE, QEI_INTDIR);
		//Reset Position
		ROM_QEIPositionSet(QEI0_BASE, 250000);
	}
}

//*****************************************************************************
//
// This example demonstrates how to setup the PWM block to generate signals.
//
//*****************************************************************************
int
main(void)
{

	unsigned char temp[5];
	volatile int32_t temp0,temp1,temp2;
	unsigned char i;
	a0=0.1;
	a1=0;
	a2=0;
	lSetVelocitypps = -300;
	lCurrentVelocitypps = 0;

    //
    // Set the clocking to run directly from the crystal.
    //
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_8MHZ);

	QEICycle=ROM_SysCtlClockGet()/10;

    //
    // Enable processor interrupts.
    //
    IntMasterEnable();

    PWM0_Init();
    UART0_Init();
    QEI_Init();



    //
    // Loop forever while the PWM signals are generated.
    //
    while(1)
    {
    	if(HWREGBITB(&ucRunningStatus,ReceiveUpdateData) == 1)
    	{
    		ROM_IntDisable(UART0_BASE);
    		HWREGBITB(&ucRunningStatus, ReceiveUpdateData) = 0;
    		for(i=0;i<5;i++)
    		{
    			temp[i]=ROM_UARTCharGet(UART0_BASE);
    		}
    		UARTSend(temp,5);
    		temp1=(temp[4]-0x30)+(temp[3]-0x30)*10+(temp[2]-0x30)*100+(temp[1]-0x30)*1000;
    		if(temp[0] == '-')
    			temp1=-temp1;
    		PWM0_ChangeDytyCycle(temp1);
    		ROM_UARTIntClear(UART0_BASE, 0x07f0);
    		ROM_IntEnable(UART0_BASE);
    	}
    	if(HWREGBITB(&ucRunningStatus,StartMotor) == 1)
    	{
    		UARTCharPut(UART0_BASE,'c');
    	    ROM_UARTIntDisable(UART0_BASE, UART_INT_RX|UART_INT_RT);
    		temp2=UARTCharGet(UART0_BASE);	//Motor Direction
    		temp1=UARTCharGet(UART0_BASE);	//MSB SetV
    		temp0=UARTCharGet(UART0_BASE);	//LSB SetV
    	    ROM_UARTIntEnable(UART0_BASE, UART_INT_RX|UART_INT_RT);
    		if(temp2=='+')
    			lSetVelocitypps=(temp1*256)+temp0;
    		else if(temp2=='-')
    			lSetVelocitypps=-((temp1*256)+temp0);
    		HWREGBITB(&ucRunningStatus,StartMotor) = 0;
    	}
    	if(HWREGBITB(&ucRunningStatus,StopMotor) == 1)
    	{
    		lSetVelocitypps=0;
    		HWREGBITB(&ucRunningStatus,StopMotor) = 0;
    	}
    	if(HWREGBITB(&ucRunningStatus,SendCVToPC) == 1)
    	{
    		ROM_UARTIntDisable(UART0_BASE, UART_INT_RX|UART_INT_RT);
			//send current velocity to computer using UART1
			UARTSendInt32(lCurrentVelocitypps);
			UARTCharPut(UART0_BASE,'\n');

			ROM_UARTIntEnable(UART0_BASE, UART_INT_RX|UART_INT_RT);
			HWREGBITB(&ucRunningStatus,SendCVToPC) = 0;
    	}
    }
}
